package com.lyfam.game.behaviosr.steering;

import java.util.ArrayList;
import java.util.Collections;

import com.lyfam.game.geometry.Vector2D;

public class SteeringEngine
{
	private ArrayList<ISteeringBehavior> steeringBehaviors;
	
	public SteeringEngine()
	{
		steeringBehaviors = new ArrayList<ISteeringBehavior>();
	}

	public void addBehavior(ISteeringBehavior behavior)
	{
		steeringBehaviors.add(behavior);
	}
	
	public Vector2D calculate(float maxForce)
	{
		Vector2D force = new Vector2D(0,0);
	    Collections.sort(steeringBehaviors);
	    for (ISteeringBehavior steer : steeringBehaviors)
	    {
	    	if (!accummulateForce(force, steer.getSteeringForce(), maxForce))
	    	{
	    		return force;
	    	}
		}
	    
	    return force;
	}
	
	private boolean accummulateForce(Vector2D finalForce, Vector2D forceToAdd, float maxForce)
	{
		// How much force has been issued so far
		float magnitudeSoFar = finalForce.getLength();
		
		// calculate how much steering force can be applied
		float magnitudeRemain = maxForce - magnitudeSoFar;
		
		if (magnitudeRemain <= 0.0f)
		{
			return false;
		}
		
		float magnitudeToAdd = forceToAdd.getLength();
		if (magnitudeToAdd < magnitudeRemain)
		{
			finalForce.add(forceToAdd);
		}
		else
		{
			Vector2D force = Vector2D.getNormalize(forceToAdd);
			force.mult(magnitudeRemain);
			finalForce.add(force);
		}
		
		return true;
	}
}
